#include <stdio.h>
#include <string.h>
#include <stdint.h>

#include "aos/hal/can.h"
#include "board.h"

#include "ulog/ulog.h"
#include "canbus.h"
#include "lidar.h"
#include "aos/cli.h"

#define LOG_TAG "LIDAR_CE30"

#define F_CE30_CMD_ID0   (0x606)
#define F_CE30_CFG_ID0   (0x607)
#define F_CE30_DATA_ID0  (0x586)
#define F_CE30_HEART_ID0 (0x587)

#define F_CE30_CMD_ID1   (0x608)
#define F_CE30_CFG_ID1   (0x609)
#define F_CE30_DATA_ID1  (0x588)
#define F_CE30_HEART_ID1 (0x589)

#define F_CE30_BAUD      (250)  //250k


#define F_TF_DATA_TX_ID (0x188)
#define F_TF_DATA_RX_ID (0x88)

#define F_CMD_LENGTH (5)
#define F_DATA_LENGTH ()
#define F_HEARTBEAT_LENGTH (3)

#define DATA_FRAME   (0)
#define REMOTE_FRAME (1)

#define F_CMD_RESERVE (0x11 << 6)
#define FRAME_RATE  (0 << 1) //20fps
#define LIDAR_OPEN  (1)
#define LIDAR_CLOSE (0)

#define OPEN_LIDAR_CMD (F_CMD_RESERVE | FRAME_RATE | LIDAR_OPEN)
#define ClOSE_LIDAR_CMD (F_CMD_RESERVE | FRAME_RATE | LIDAR_CLOSE)

#define DATA_SIZE_MAX (16)

//static int perfect = PERFECT_VAL_DEFAULT;

typedef struct {
	uint32_t status:1;
	uint32_t ver_invalid:1;
	uint32_t error:1;
	uint32_t reserver:1;
	uint32_t is_central_correction:1;
	uint32_t heartbeat_value:3;
	uint32_t ver:16;
} S_HEARTBEAT_FRAME;

typedef struct {
	uint8_t cmd;   
	/*	bit6-7:0x11 
		bit5-1:frame rate 
		bit0:start(1) stop(0)
	*/
	uint8_t spare; //reserved as 0x00
	uint8_t width;
	uint8_t high;
	uint8_t expand_cmd;
} S_CMD_FRAME;

typedef struct {
	uint16_t dist;
	uint8_t reserve0;
	int8_t angle;
	uint8_t reserve1;
	uint8_t reserve2;
	uint8_t reserve3;
	uint8_t valid;
} S_DATA_FRAME;

canbus_dev_t dev_ht, dev_dt, dev_dt_e; 
static int log_flag_ht = 0;
static int log_flag_dt = 0, log_flag_dt_e = 0;

//extern int16_t f_dist_l;
static int16_t f_dist;

int16_t get_f_dist(void)
{
	return f_dist;
}

static int lidar_ce30_send(can_frame_t *frm) \
{
	if (NULL == frm ||
		NULL == dev_ht.bus ||
		NULL == dev_ht.bus->canbus_send) {
		return -1;
	}
	return dev_ht.bus->canbus_send(frm);
}


int open_lidar(canbus_dev_t *dev, uint32_t id)
{
	can_frame_t start_frame;
	S_CMD_FRAME start_cmd_t;

	start_frame.header.id = id;
	start_frame.header.dlc = F_CMD_LENGTH;
	start_frame.header.rtr = DATA_FRAME;

	start_cmd_t.cmd = OPEN_LIDAR_CMD;
	start_cmd_t.spare = 0;
	start_cmd_t.high = 0x23;
	start_cmd_t.width = 0x28;
	start_cmd_t.expand_cmd = 0;

	memcpy(start_frame.data, &start_cmd_t, start_frame.header.dlc);

	return lidar_ce30_send(&start_frame);

}


int close_lidar(canbus_dev_t *dev, uint32_t id)
{
	
	can_frame_t start_frame;
	S_CMD_FRAME start_cmd_t;

	start_frame.header.id = id;
	start_frame.header.dlc = F_CMD_LENGTH;
	start_frame.header.rtr = DATA_FRAME;

	start_cmd_t.cmd = ClOSE_LIDAR_CMD;
	start_cmd_t.spare = 0;
	start_cmd_t.high = 0;
	start_cmd_t.width = 0;
	start_cmd_t.expand_cmd = 0;

	memcpy(start_frame.data, start_cmd_t.cmd, start_frame.header.dlc);

	return lidar_ce30_send(&start_frame);
}

int change_id_lidar(canbus_dev_t *dev, int id)
{
	can_frame_t start_frame;
	uint64_t set = 0;
	uint8_t *pdata = start_frame.data;

	start_frame.header.id = id;
	start_frame.header.dlc = 8;
	start_frame.header.rtr = DATA_FRAME;

	pdata[7] = (F_CE30_BAUD & 0xFF00) >> 8;
	pdata[6] = F_CE30_BAUD & 0xFF;
	pdata[5] = (F_CE30_CFG_ID1 & 0xFF0) >> 4;
	pdata[4] = ((F_CE30_HEART_ID1 & 0xF00) >> 8) | ((F_CE30_CFG_ID1 & 0xF) << 4);
	pdata[3] = F_CE30_HEART_ID1 & 0xFF;
	pdata[2] = (F_CE30_CMD_ID1 & 0xFF0) >> 4;
	pdata[1] = ((F_CE30_DATA_ID1 & 0xF00) >> 8) | ((F_CE30_CMD_ID1 & 0xF) << 4);
	pdata[0] = F_CE30_DATA_ID1 & 0xFF;
	/*
	set = (((F_CE30_BAUD & 0xFFFF) << 48) | 
		  ((F_CE30_CFG_ID1 & 0xFFF) << 36) |
		  ((F_CE30_HEART_ID1 & 0xFFF) << 12) |
		  ((F_CE30_CMD_ID1 & 0xFFF) << 24) |
		  (F_CE30_DATA_ID1 & 0xFFF));

	memcpy(start_frame.data, &set, start_frame.header.dlc);
	printf("idd:%lx\r\n", set);*/
	printf("idd:%x %x %x %x %x %x %x %x\r\n", pdata[0], pdata[1], pdata[2], pdata[3], pdata[4], pdata[5], pdata[6], pdata[7]);

	return lidar_ce30_send(&start_frame);

}


static void lidar_cmd_routine(char *buf, int len, int argc, char **argv)
{
	canbus_dev_t *dev = &dev_ht;
	int arg1 = strtoul(argv[2], NULL, 10);
    int arg2 = strtoul(argv[3], NULL, 16);

	switch (*(argv[1]))
	{
	case 's':
		aos_cli_printf("set id %x\r\n", arg2);
		
		change_id_lidar(dev, arg2);
		break;
	case 'o':
	   aos_cli_printf("open lidar id %d\r\n", arg2);
	   if (arg2 == 0){
			arg2 = F_CE30_CMD_ID0;
	   }else{
			arg2 = F_CE30_CMD_ID1;
	   }

	   open_lidar(dev, arg2);
	   break;

	case 'c':
	   aos_cli_printf("close lidar\r\n");
	   close_lidar(dev, arg2);
	   break;
	case 'p':
		if (*(argv[2]) == 'h') {
			aos_cli_printf("printf heart log[%d]\r\n", arg2);
			log_flag_ht = arg2;
		}
		else if (*(argv[2]) == 'd'){
			aos_cli_printf("printf data log[%d]\r\n", arg2);
			log_flag_dt = arg2;
		}
		else if (*(argv[2]) == 'e'){
			aos_cli_printf("printf ex log[%d]\r\n", arg2);
			log_flag_dt_e = arg2;
		}
		break;
	default:
	   aos_cli_printf("lidar Unknown cs %d\r\n", *(argv[1]));
	   break;
	}
}


static struct cli_command lidar_cmd[] = {
    { "lidar","control or get status of lidar", lidar_cmd_routine },
};

static void lidar_ce30_ht_routine()
{
	int ret = -1;
	canbus_dev_t *dev = &dev_ht;

	dev->rx_buf = (uint8_t *)aos_malloc(DATA_SIZE_MAX);
	dev->rx_id = F_CE30_HEART_ID0;
	ret = canbus_dev_register(dev);
	if (ret < 0) {
		aos_free(dev->rx_buf);
		LOGE(LOG_TAG, "dev lidarce30 register failed!");
		return ;
	}
	
	while(1){
		if (dev->sem_t == NULL) {
			aos_msleep(1000);
			continue;
		}
		if (aos_sem_wait((aos_sem_t *)&dev->sem_t, AOS_WAIT_FOREVER) != 0){
			continue;
		}

		S_HEARTBEAT_FRAME * pheartbeat = (S_HEARTBEAT_FRAME *)dev->rx_buf;
		if (log_flag_ht){
			
			LOGI(LOG_TAG, "status:     %s", pheartbeat->status?"start":"stop");
			LOGI(LOG_TAG, "ver_inval:  %s", pheartbeat->ver_invalid?"yes":"no");
			LOGI(LOG_TAG, "error:      %s", pheartbeat->error?"yes":"no");
			LOGI(LOG_TAG, "is_cent_cr: %s", pheartbeat->is_central_correction?"yes":"no");
			LOGI(LOG_TAG, "ver:        %d", pheartbeat->ver);
		}
		//aos_msleep(1000);
	}
}

static void lidar_ce30_data_routine()
{
	int ret = -1;
	canbus_dev_t *dev = &dev_dt;

	dev->rx_buf = (uint8_t *)aos_malloc(DATA_SIZE_MAX);
	dev->rx_id = F_CE30_DATA_ID0;

	ret = canbus_dev_register(dev);
	if (ret < 0) {
		aos_free(dev->rx_buf);
		LOGE(LOG_TAG, "dev lidarce30 register failed!");
		return ;
	}
	
	while(1){
		if (dev->sem_t == NULL) {
			aos_msleep(1000);
			continue;
		}
		if (aos_sem_wait((aos_sem_t *)&dev->sem_t, 1000) != 0){
			f_dist = 0;
			continue;
		}
		S_DATA_FRAME *pdata = (S_DATA_FRAME *)dev->rx_buf;
		//f_dist_l = f_dist;
		f_dist = pdata->dist;
		if (f_dist > 300)
		{
			f_dist = 0;
		}
		if (pdata->valid && log_flag_dt)
		{
			printf("dist %d\r\n", pdata->dist);
			printf("angle %d\r\n", pdata->angle);
		}
		//aos_msleep(1000);

	}
}

static void lidar_ce30_e_data_routine()
{
	int ret = -1;
	canbus_dev_t *dev = &dev_dt_e;

	dev->rx_buf = (uint8_t *)aos_malloc(DATA_SIZE_MAX);
	dev->rx_id = F_CE30_DATA_ID1;

	ret = canbus_dev_register(dev);
	if (ret < 0) {
		aos_free(dev->rx_buf);
		LOGE(LOG_TAG, "dev lidarce30 register failed!");
		return ;
	}
	
	while(1){
		if (dev->sem_t == NULL) {
			aos_msleep(1000);
			continue;
		}
		if (aos_sem_wait((aos_sem_t *)&dev->sem_t, 1000) != 0){
			f_dist = 0;
			continue;
		}
		S_DATA_FRAME *pdata = (S_DATA_FRAME *)dev->rx_buf;
		//f_dist_l = f_dist;
		f_dist = pdata->dist;
		if (f_dist > 300)
		{
			f_dist = 0;
		}
		if (pdata->valid && log_flag_dt_e)
		{
			printf("dist e %d\r\n", pdata->dist);
			printf("angle e %d\r\n", pdata->angle);
		}
		//aos_msleep(1000);

	}
}


int init_lidar_ce30()
{

	if (0 != aos_task_new("lidar_ce30_ht", lidar_ce30_ht_routine, NULL, CAN_DEMO_TASK_DEPTH)) {
		LOGE(LOG_TAG, "create ce30_ht_routine fail");
	}

	if (0 != aos_task_new("lidar_ce30_data", lidar_ce30_data_routine, NULL, CAN_DEMO_TASK_DEPTH)) {
		LOGE(LOG_TAG, "create ce30_data_routine fail");
	}
/*
	if (0 != aos_task_new("lidar_ce30_data_e", lidar_ce30_e_data_routine, NULL, CAN_DEMO_TASK_DEPTH)) {
		LOGE(LOG_TAG, "create ce30_data_routine fail");
	}
*/
	open_lidar(&dev_dt, F_CE30_CMD_ID0);

	aos_cli_register_commands(&lidar_cmd[0], sizeof(lidar_cmd) / sizeof(struct cli_command));
	
}


void lidar_opt(int v)
{
	if (v == 1){
		open_lidar(&dev_dt, F_CE30_CMD_ID0);
	}else{
		close_lidar(&dev_dt, F_CE30_CMD_ID0);
	}
}

